﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using Microsoft.Kinect.Interop;
using KinectPG.KinectGameLibrary.Enums;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework;

namespace KinectPG.KinectGameLibrary
{
    public class SkeletonManager
    {
        #region Fields
        private TransformSmoothParameters _smoothParams;
        private KinectSensor _sensor;
        private Limb[] _limbs;
        private GraphicsDevice _graphicsDevice;
        private int DefaultWidth = 640;
        private int DefaultHeight = 480;
        private Skeleton skeleton;
        private Skeleton[] skeletons = new Skeleton[1];
        private SkeletonFrame allSkeletons;
        private ColorImageFrame colorFrame;
        private DepthImageFrame depthFrame;
        private byte[] colorFramePixels;
        private Texture2D videoFrame;
        private Texture2D videoFrame2, colorVideo;
        public Point skelPoint;
        #endregion

        #region Properties
        public string KinectName
        {
            get
            {
                if (_sensor != null) return _sensor.UniqueKinectId;
                return "No Kinect device connected!";
            }
        }

        public string KinectStatus
        {
            get
            {
                if (_sensor != null) return _sensor.Status.ToString();
                return "No Kinect device connected";
            }
        }

        public bool KinectConnected
        {
            get
            {
                if (_sensor != null)
                {
                    if (_sensor.Status == Microsoft.Kinect.KinectStatus.Connected) return true;
                }
                return false;
            }
        }

        public int ScreenWidth
        {
            get
            {
                if (_graphicsDevice != null) return _graphicsDevice.DisplayMode.Width;
                return DefaultWidth;
            }
        }

        public int ScreenHeight
        {
            get
            {
                if (_graphicsDevice != null) return _graphicsDevice.DisplayMode.Height;
                return DefaultHeight;
            }
        }

        public Texture2D ColorVideo
        {
            get
            {
                return colorVideo;
            }
        }

        public Limb[] Limbs
        {
            get
            {
                return _limbs;
            }
        }
        #endregion

        #region Constructors

        public SkeletonManager(TransformSmoothParametersEnum param, GraphicsDevice graphicsDevice)
        {
            foreach (KinectSensor sensor in KinectSensor.KinectSensors)
            {
                _sensor = sensor;
                break;
            }

            switch (param)
            {
                case TransformSmoothParametersEnum.VerySmooth:
                    _smoothParams.Smoothing = 0.4f;
                    _smoothParams.Correction = 0.2f;
                    _smoothParams.Prediction = 0.15f;
                    _smoothParams.JitterRadius = 1.0f;
                    _smoothParams.MaxDeviationRadius = 0.5f;
                    break;
                case TransformSmoothParametersEnum.Smooth:
                    _smoothParams.Smoothing = 0.4f;
                    _smoothParams.Correction = 0.2f;
                    _smoothParams.Prediction = 0.15f;
                    _smoothParams.JitterRadius = 1.0f;
                    _smoothParams.MaxDeviationRadius = 0.5f;
                    break;
                case TransformSmoothParametersEnum.Scratchy:
                    _smoothParams.Smoothing = 0.4f;
                    _smoothParams.Correction = 0.2f;
                    _smoothParams.Prediction = 0.15f;
                    _smoothParams.JitterRadius = 1.0f;
                    _smoothParams.MaxDeviationRadius = 0.5f;
                    break;
            }


            //_sensor.SkeletonStream.Enable();
            _sensor.SkeletonStream.Enable(_smoothParams);
            _sensor.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(_sensor_SkeletonFrameReady);
            //_sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
            //_sensor.ColorFrameReady += new EventHandler<ColorImageFrameReadyEventArgs>(_sensor_ColorFrameReady);
            _sensor.DepthStream.Enable();
            _sensor.DepthFrameReady += new EventHandler<DepthImageFrameReadyEventArgs>(_sensor_DepthFrameReady);
            _sensor.Start();
            _limbs = new Limb[20];
            for (int i = 0; i < 20; i++)
                _limbs[i] = new Limb((JointType)Enum.GetValues(typeof(JointType)).GetValue(i));

            _graphicsDevice = graphicsDevice;
        }

        void _sensor_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        {
            this.depthFrame = e.OpenDepthImageFrame();
        }



        #endregion

        void _sensor_ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
        {
            colorFrame = e.OpenColorImageFrame();

            if (colorFrame != null)
            {
                //Create array for pixel data and copy it from the image frame
                Byte[] pixelData = new Byte[colorFrame.PixelDataLength];
                colorFrame.CopyPixelDataTo(pixelData);
                colorVideo = new Texture2D(_graphicsDevice, colorFrame.Width, colorFrame.Height);

                colorFramePixels = new byte[colorFrame.PixelDataLength];
                colorFrame.CopyPixelDataTo(colorFramePixels);

                Byte[] bgraPixelData = new Byte[colorFrame.PixelDataLength];
                for (int i = 0; i < colorFramePixels.Length; i += 4)
                {
                    bgraPixelData[i] = colorFramePixels[i + 2];
                    bgraPixelData[i + 1] = colorFramePixels[i + 1];
                    bgraPixelData[i + 2] = colorFramePixels[i];
                    bgraPixelData[i + 3] = (Byte)255; //The video comes with 0 alpha so it is transparent
                }

                colorVideo = new Texture2D(_graphicsDevice, colorFrame.Width, colorFrame.Height);
                colorVideo.SetData(bgraPixelData);

            }
            


            try
            {
                colorFramePixels = new byte[colorFrame.PixelDataLength];
                colorFrame.CopyPixelDataTo(colorFramePixels);
                videoFrame = new Texture2D(_graphicsDevice, colorFrame.Width, colorFrame.Height);
                videoFrame.SetData(colorFramePixels);
                videoFrame2 = videoFrame;
            }
            catch { }
        }

        private Point GetPosition2DLocation(DepthImageFrame depthFrame, SkeletonPoint skeletonPoint)
        {
            if(depthFrame==null) return new Point(0,0);
            DepthImagePoint depthPoint = depthFrame.MapFromSkeletonPoint(skeletonPoint);

                    ColorImagePoint colorPoint = depthFrame.MapToColorImagePoint(depthPoint.X, depthPoint.Y, this._sensor.ColorStream.Format);

                    // map back to skeleton.Width & skeleton.Height
                    return new Point(
                        (int)(_graphicsDevice.DisplayMode.Width * colorPoint.X / this._sensor.ColorStream.FrameWidth),
                        (int)(_graphicsDevice.DisplayMode.Height * colorPoint.Y / this._sensor.ColorStream.FrameHeight));
                
            
        }

        private void _sensor_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            allSkeletons = e.OpenSkeletonFrame();

            skeleton = null;

            if(allSkeletons!=null) {

                if(skeletons.Length != allSkeletons.SkeletonArrayLength)
                    skeletons = new Skeleton[allSkeletons.SkeletonArrayLength];

                allSkeletons.CopySkeletonDataTo(skeletons);

                skeleton = (from s in skeletons
                                     where s.TrackingState == SkeletonTrackingState.Tracked
                                     select s).FirstOrDefault();

                if (skeleton != null)
                {


                    for (int i = 0; i < 20; i++)
                    {
                        _limbs[i].Z = skeleton.Joints[(JointType)Enum.GetValues(typeof(JointType)).GetValue(i)].Position.Z;
                        _limbs[i].X = (skeleton.Joints[(JointType)Enum.GetValues(typeof(JointType)).GetValue(i)].Position.X);
                        _limbs[i].Y = (skeleton.Joints[(JointType)Enum.GetValues(typeof(JointType)).GetValue(i)].Position.Y);
                        SkeletonPoint pt = new SkeletonPoint() { X = (float)_limbs[i].X, Y = (float)_limbs[i].Y, Z = (float)_limbs[i].Z };
                        if (this.depthFrame != null)
                        {
                            skelPoint = this.GetPosition2DLocation(depthFrame, pt);
                            _limbs[i].ScreenPoint = skelPoint;
                        }
                    }
                    
                }
            }
        }

        public Texture2D GetVideoFrame()
        {
            if (videoFrame2 == null)
            {
                return null;
            }
            else return videoFrame2;
        }

        public double GetLimbX(JointType limbEnum)
        {
            return _limbs[(int)limbEnum].X;
        }

        public double GetLimbY(JointType limbEnum)
        {
            return _limbs[(int)limbEnum].Y;
        }

        public double GetLimbZ(JointType limbEnum)
        {
            return _limbs[(int)limbEnum].Z;
        }

    }
}
